#include "widget.h"
#include "config.h"

/**********************
将数据保留4位小数
**********************/
inline double Decimal_Places(double x)
{
    x *= 1e4;
    x = ROUND(x);
    return x/1e4;
}

/**********************
100ms定时事件
端口扫描,处理串口接收缓冲区,发送目标步数和目标位姿
**********************/
void Widget::Slot_tmr100ms_Tick()
{
    static u8 TimerCnt = 0;
    TimerCnt++;
    _t += 0.1;
#ifdef USE_QTCHART
    line1->append(_t,sin(_t));
    line1->remove(0);
    chart->scroll(10, 0);
#endif
    if (TimerCnt >= 5)
        SerialPort_Scan();
    if (TimerCnt >= 50) {
        MESSAGE_NORMAL();
        TimerCnt = 0;
    }
    if (ar3._isworking) Trajectory_Update();
    if (ckbx3Options[0]->isChecked() && ckbx1AutoSend->isChecked())
        Security_Check();
#ifdef USE_OPENCV
    if (vision.ImgShow){
        if (vision.ImgShow_Update(ckbx4Options[1]->isChecked())){
            MESSAGE_ERROR("相机读取图像失败!");
            vision.ImgShow = !vision.ImgShow;
            vision.ImgShow_Exit();
        }
        else{
            QString str = QString::number(vision.cursorx) + ",";
            str += QString::number(vision.cursory) + ",";
            lbl4Coordinates[5]->setText(str);
            if (vision.clicked){
                tedt5clipboard->setText(tedt5clipboard->toPlainText() + str);
                vision.clicked = false;
            }
        }
    }
#endif
    if (!serialPort->isOpen()) return;
    SerialPort_Receive();
    if (!ckbx1AutoSend->isChecked()) return;
    serialPort->Send_s16_Data(PulseTargetTab1, 0x01, 6);
}

/**********************
串口开闭按钮点击事件
**********************/
void Widget::Slot_btnOpen_Click()
{
    if (serialPort->isOpen())
        SerialPort_Close();
    else {
        if (cbbxPort->currentText() == "") return;
        if (cbbxBaudrate->currentText() == "") return;
        serialPort->setPortName(cbbxPort->currentText());
        serialPort->setBaudRate(cbbxBaudrate->currentText().toInt());
        if (serialPort->open(QIODevice::ReadWrite))
            SerialPort_Open();
        else
            MESSAGE_ERROR("串口打开失败!");
    }
}

/**********************
紧急停止按钮点击事件
**********************/
void Widget::Slot_btnEmergencyStop_Click()
{
    if (!serialPort->isOpen()) return;
    ckbx1AutoSend->setChecked(false);
    if (ckbxEmergency->isChecked())
        serialPort->Send_u8_Data(0xC0, PULSE_RESET);
    else
        serialPort->Send_u8_Data(0x80, PULSE_RESET);
}

/**********************
标签1:移动任何一个拖动条都实时计算最终目标步数与目标角度
**********************/
void Widget::Slot_slid1Tuning_valueChanged()
{
    QString name = sender()->objectName();
    int n = name.toInt();
    PulseTargetTab1[n] = s16(slid1Tuning1[n]->value() * 100);
    PulseTargetTab1[n] += slid1Tuning2[n]->value() * 10;
    PulseTargetTab1[n] += slid1Tuning3[n]->value();
    if(ckbx2Reverse[n]->isChecked())
        PulseTargetTab1[n] = -PulseTargetTab1[n];
    ledt1PulseTarget[n]->setText(QString::number(PulseTargetTab1[n]));
    JointAngleTab1[n] = PulseTargetTab1[n] * ar3.DriveRatio[n] + ar3.NegRadLim[n];
    lbl1JointAngle[n]->setNum(Decimal_Places(JointAngleTab1[n]));
    ar3.Forward_Solve(JointAngleTab1, PostureTab1);
    for (int i = 0; i < 6; ++i)
        lbl1Posture[i]->setNum(Decimal_Places(PostureTab1[i]));
}

/**********************
标签1:点击移动到固定位置按钮
**********************/
void Widget::Slot_btn1FixLocation_Click()
{
    s16 pulse[6];
    s16 pulsetemp;
    bool correct;
    ar3._isworking = false;
    switch(sender()->objectName().toInt()) {
    case 0:for(int i=0;i<5;++i)pulse[i]=0;pulse[5]=-1200;break;
    case 1:for(int i=0;i<6;++i)pulse[i]=ar3.PulseCenter[i];break;
    case 2:for(int i=0;i<6;++i)pulse[i]=ar3.PulseWorking[i];break;
    case 3:for(int i=0;i<6;++i)pulse[i]=ar3.PulseSleep[i];break;
    case 4:
        for(int i=0;i<6;++i)
            pulse[i]=s16(ledt1PulseTarget[i]->text().toInt(&correct));
        pulse[1] = 10;
        break;
    case 5:
        for(int i=0;i<6;++i){
            pulse[i] = s16(ledt1PulseTarget[i]->text().toInt(&correct));
            if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
            if (pulse[i]>=11100) { MESSAGE_ERROR("数据越界!"); return; }
        }break;
    default:return;
    }
    for(int i=0;i<6;++i) {
        if (pulse[i]>=0) {
            pulsetemp = pulse[i];
            ckbx2Reverse[i]->setChecked(false);
        }
        else {
            pulsetemp = -pulse[i];
            ckbx2Reverse[i]->setChecked(true);
        }
        slid1Tuning1[i]->setValue(pulsetemp/100);
        slid1Tuning2[i]->setValue((pulsetemp%100)/10);
        slid1Tuning3[i]->setValue(pulsetemp%10);
        emit slid1Tuning1[i]->valueChanged(slid1Tuning1[i]->value());
    }
}

/**********************
标签2:点击反向勾选框后:
目标步数自动置零
取消勾选实时发送(标签1)勾选框
取消勾选逆解拖动(标签3)按钮
**********************/
void Widget::Slot_ckbx2Reverse_Click()
{
    int n = sender()->objectName().toInt();
    slid1Tuning1[n]->setValue(0);
    slid1Tuning2[n]->setValue(0);
    slid1Tuning3[n]->setValue(0);
    ckbx1AutoSend->setChecked(false);
}

/**********************
标签2:校准按钮点击事件
**********************/
void Widget::Slot_btn2JointCalibrate_Click()
{
    if (!serialPort->isOpen()) return;
    ckbx1AutoSend->setCheckState(Qt::Unchecked);
    int n = sender()->objectName().toInt();
    if (n == 0) {
        serialPort->Send_u8_Data(0x7C, REQUIRE);
        for (int i = 0; i < 6; ++i)
            lbl2CalibrateFlag[i]->setNum(0);
    }
    else if (n == 7)
        for (int i = 0; i < 6; ++i)
            lbl2CalibrateFlag[i]->setNum(0);
    else {
        serialPort->Send_u8_Data(1 << (n + 1), REQUIRE);
        lbl2CalibrateFlag[(n - 1)]->setNum(0);
    }
}

/**********************
标签2:步数复位按钮点击事件
**********************/
void Widget::Slot_btn2ResetStep_Click()
{
    int n = sender()->objectName().toInt();
    if (n<6){
        slid1Tuning1[n]->setValue(0);
        slid1Tuning2[n]->setValue(0);
        slid1Tuning3[n]->setValue(0);
        ckbx2Reverse[n]->setChecked(false);
        if (serialPort->isOpen())
            serialPort->Send_u8_Data(1<<n, PULSE_RESET);
    }
    else {
        for (int i = 0; i < 6; ++i){
            slid1Tuning1[i]->setValue(0);
            slid1Tuning2[i]->setValue(0);
            slid1Tuning3[i]->setValue(0);
            ckbx2Reverse[i]->setChecked(false);
        }
        if (serialPort->isOpen())
            serialPort->Send_u8_Data(0x3F, PULSE_RESET);
    }
}

/**********************
标签2:编辑框取负按钮点击事件
**********************/
void Widget::Slot_btn2ReverseStep_Click()
{
    QString s;
    ckbx1AutoSend->setChecked(false);
    for (int i = 0; i < 6; ++i){
        s = ledt1PulseTarget[i]->text();
        if (s.contains("-")) s.replace("-", "");
        else                 s = "-" + s;
        ledt1PulseTarget[i]->setText(s);
    }
    emit btn1FixLocation[5]->clicked();
    emit slid1Tuning1[0]->valueChanged(slid1Tuning1[0]->value());
}

/**********************
标签2:速度读取按钮点击事件
**********************/
void Widget::Slot_btn2ReadSpeData_Click()
{
    if (!serialPort->isOpen()) return;
    serialPort->Send_u8_Data(0x01, REQUIRE);
}

/**********************
标签2:加速度读取按钮点击事件
**********************/
void Widget::Slot_btn2ReadAccData_Click()
{
    if (!serialPort->isOpen()) return;
    serialPort->Send_u8_Data(0x02, REQUIRE);
}

/**********************
标签2:角度下限读取按钮点击事件
**********************/
void Widget::Slot_btn2ReadNegRadLim_Click()
{
    for (int i = 0; i < 6; ++i)
        ledt2NegRadLim[i]->setText(QString::number(ar3.NegRadLim[i], 'f', 4));
}

/**********************
标签2:速度写入按钮点击事件
**********************/
void Widget::Slot_btn2WriteSpeData_Click()
{
    if (!serialPort->isOpen()) { MESSAGE_ERROR("串口未打开!"); return; }
    s16 data[6];
    bool correct;
    for (int i = 0; i < 6; ++i) {
        data[i] = s16(ledt2JointSpeed[i]->text().toInt(&correct));
        if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
    }
    serialPort->Send_s16_Data(data, MAX_SPEED, 6);
    MESSAGE_NORMAL();
}

/**********************
标签2:加速度写入按钮点击事件
**********************/
void Widget::Slot_btn2WriteAccData_Click()
{
    if (!serialPort->isOpen()) { MESSAGE_ERROR("串口未打开!"); return; }
    s16 data[6];
    bool correct;
    for (int i = 0; i < 6; ++i) {
        data[i] = s16(ledt2JointAccelerate[i]->text().toInt(&correct));
        if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
    }
    serialPort->Send_s16_Data(data, MAX_ACC, 6);
    MESSAGE_NORMAL();
}

/**********************
标签2:角度下限写入按钮点击事件
**********************/
void Widget::Slot_btn2WriteNegRadLim_Click()
{
    bool correct;
    double paramtemp;
    for (int i = 0; i < 6; ++i) {
        paramtemp = ledt2NegRadLim[i]->text().toDouble(&correct);
        if (!correct) continue;
        ar3.NegRadLim[i] = paramtemp;
    }
}

/**********************
标签3:位姿增量按钮
**********************/
void Widget::Slot_btn3PostureDelta_Click()
{
    bool correct;
    int n = sender()->objectName().toInt();
    double datatemp = ledt3PostureDeltaValue[n%6]->text().toDouble(&correct);
    if (!correct) { MESSAGE_ERROR("数据格式不正确1!"); return; }
    double pulsetemp = ledt3Posture[n%6]->text().toDouble(&correct);
    if (!correct) { MESSAGE_ERROR("数据格式不正确2!"); return; }
    pulsetemp = (n/6) ? (pulsetemp-datatemp) : (pulsetemp+datatemp);
    ledt3Posture[n%6]->setText(QString::number(pulsetemp, 'g', 8));
    if (!ckbx1AutoSend->isChecked()) return;
    Slot_btn3Reverse_Click();
    Slot_btn3SynchroniseStep_Click();
}

/**********************
标签3:统一增加位姿按钮
**********************/
void Widget::Slot_btn3PostureDeltaPlus6_Click()
{
    bool correct;
    double datatemp, postemp;
    for (int i = 0; i < 6; ++i){
        datatemp = ledt3PostureDeltaValue[i]->text().toDouble(&correct);
        if (!correct) { MESSAGE_ERROR("数据格式不正确1!"); return; }
        postemp = ledt3Posture[i]->text().toDouble(&correct);
        if (!correct) { MESSAGE_ERROR("数据格式不正确2!"); return; }
        ar3._start[i] = postemp;
        postemp += datatemp;
        ar3._end[i] = postemp;
        ledt3Posture[i]->setText(QString::number(postemp, 'g', 8));
    }
    if (!ckbx1AutoSend->isChecked()) return;
    if (!ckbx3Options[3]->isChecked()) return;
    Trajectory_Init();
}

/**********************
标签3:统一减少位姿按钮
**********************/
void Widget::Slot_btn3PostureDeltaMinus6_Click()
{
    bool correct;
    double datatemp, postemp;
    for (int i = 0; i < 6; ++i){
        datatemp = ledt3PostureDeltaValue[i]->text().toDouble(&correct);
        if (!correct) { MESSAGE_ERROR("数据格式不正确1!"); return; }
        postemp = ledt3Posture[i]->text().toDouble(&correct);
        if (!correct) { MESSAGE_ERROR("数据格式不正确2!"); return; }
        ar3._start[i] = postemp;
        postemp -= datatemp;
        ar3._end[i] = postemp;
        ledt3Posture[i]->setText(QString::number(postemp, 'g', 8));
    }
    if (!ckbx1AutoSend->isChecked()) return;
    if (!ckbx3Options[3]->isChecked()) return;
    Trajectory_Init();
}

/**********************
标签3:步数增量按钮
**********************/
void Widget::Slot_btn3PulseDelta_Click()
{
    bool correct;
    int n = sender()->objectName().toInt();
    bool minus = n/6;
    n %= 6;
    int datatemp = ledt3PulseDeltaValue[n]->text().toInt(&correct);
    if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
    int pulsetemp = ledt1PulseTarget[n]->text().toInt(&correct);
    if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
    pulsetemp = (minus) ? (pulsetemp-datatemp) : (pulsetemp+datatemp);
    if (pulsetemp<0){
        ckbx2Reverse[n]->setChecked(true);
        pulsetemp = -pulsetemp;
    }
    slid1Tuning1[n]->setValue(pulsetemp/100);
    slid1Tuning2[n]->setValue((pulsetemp%100)/10);
    slid1Tuning3[n]->setValue(pulsetemp%10);
}

/**********************
标签3:逆解按钮点击事件
**********************/
void Widget::Slot_btn3Reverse_Click()
{
    double datatemp, roundtemp;
    bool correct;
    for (int i = 0; i < 6; ++i) {
        datatemp = ledt3Posture[i]->text().toDouble(&correct);
        if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
        PostureTab3[i] = datatemp;
        lbl3Posture[i]->setNum(PostureTab3[i]);
    }
    if(ar3.Inverse_Solve(PostureTab3, JointAngleTab3) != 0){
        MESSAGE_ERROR("逆解不存在!");
        return;
    }
    for (int i = 0; i < 6; ++i) {
        if (i==5 && ckbx3Options[2]->isChecked()){
            PulseTargetTab3[i] = PulseTargetTab1[i];
            ledt3PulseTarget[i]->setText(ledt1PulseTarget[i]->text());
            lbl3JointAngle[i]->setText(lbl1JointAngle[i]->text());
            break;
        }
        roundtemp = Decimal_Places(JointAngleTab3[i]);
        lbl3JointAngle[i]->setNum(roundtemp);
        roundtemp = (JointAngleTab3[i] - ar3.NegRadLim[i]) / ar3.DriveRatio[i];
        PulseTargetTab3[i] = ROUND(roundtemp);
        if(PulseTargetTab3[i] < 0)
            ckbx2Reverse[i]->setChecked(true);
        else
            ckbx2Reverse[i]->setChecked(false);
        ledt3PulseTarget[i]->setText(QString::number(PulseTargetTab3[i]));
    }
    MESSAGE_NORMAL();
}

/**********************
标签3:步数同步按钮点击事件
**********************/
void Widget::Slot_btn3SynchroniseStep_Click()
{
    int pulse;
    for(int i=0;i<6;++i){
        if (i==5 && ckbx3Options[2]->isChecked()) continue;
        if(PulseTargetTab3[i] < 0)
            ckbx2Reverse[i]->setChecked(true);
        else
            ckbx2Reverse[i]->setChecked(false);
        pulse = ABS(PulseTargetTab3[i]);
        slid1Tuning1[i]->setValue(pulse/100);
        slid1Tuning2[i]->setValue((pulse%100)/10);
        slid1Tuning3[i]->setValue(pulse%10);
    }
}

/**********************
标签3: 读取末端点坐标按钮点击事件
**********************/
void Widget::Slot_btn3ReadEff_Click()
{
    ledt3Eff[0]->setText(QString::number(ar3._Eff._x));
    ledt3Eff[1]->setText(QString::number(ar3._Eff._y));
    ledt3Eff[2]->setText(QString::number(ar3._Eff._z));
}

/**********************
标签3: 写入末端点坐标按钮点击事件
**********************/
void Widget::Slot_btn3WriteEff_Click()
{
    double datatemp;
    bool correct;
    datatemp = ledt3Eff[0]->text().toDouble(&correct);
    if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
    ar3._Eff._x = datatemp;
    datatemp = ledt3Eff[1]->text().toDouble(&correct);
    if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
    ar3._Eff._y = datatemp;
    datatemp = ledt3Eff[2]->text().toDouble(&correct);
    if (!correct) { MESSAGE_ERROR("数据格式不正确!"); return; }
    ar3._Eff._z = datatemp;
}

/**********************
标签4:用户界面按钮点击事件
**********************/
void Widget::Slot_btn4UserInterface_Click()
{
    QString name = sender()->objectName();
    KeyID = name.toInt() - 1;
    lbl4Coordinates[0]->setText(name);
}

/**********************
标签4:按压检测勾选框点击事件
**********************/
void Widget::Slot_ckbx4Pressure_Click()
{
    if (!serialPort->isOpen()) { MESSAGE_ERROR("串口未打开!"); return; }
    serialPort->Send_u8_Data(ckbx4Options[3]->isChecked(), REQ_ADC);
}

/**********************
标签4:按压数据写入勾选框点击事件
**********************/
void Widget::Slot_ckbx4WriteData_Click()
{
    if (ckbx4Options[4]->isChecked()){
        pressfile.Add_File(STRING(PRESSFILE_PATH));
        std::cout<<STRING(PRESSFILE_PATH)<<std::endl;
    }
    else
        pressfile.close();
}

#ifdef USE_OPENCV
/**********************
标签4:打开/关闭摄像头
**********************/
void Widget::Slot_ckbx4CameraTest()
{
    vision.ImgShow = ckbx4Options[0]->isChecked();
    if (vision.ImgShow){
        if(vision.ImgShow_Init()){
            MESSAGE_ERROR("没有检测到相机!");
            vision.ImgShow = !vision.ImgShow;
        }
        else
            MESSAGE_NORMAL();
    }
    else{
        vision.ImgShow_Exit();
    }
}

/**********************
标签4:添加模板图片
**********************/
void Widget::Slot_btn4AddImageFromFile()
{
    vision.Add_ImageFromFile("/home/pi/Desktop/myfile/device3.png");
}

/**********************
标签4:用户界面按键的3D定位
**********************/
void Widget::Slot_btn4Locate3d()
{
    if (vision.ImgShow){
        MESSAGE_ERROR("请先关闭相机!");
        return;
    }
    cv::VideoCapture cap(0);
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cv::Mat scene, imgshow, imgL, imgR;
    if (!cap.isOpened()) return;
    if (!cap.read(scene)) return;
    cap.release();
    int errcode;
    cv::Point2f KeyLocate;  // 按键在模板图像中的像素坐标
    cv::Point2f KeyRemapL, KeyRemapR;  // 按键在相机左右图像中的像素坐标
    KeyLocate = cv::Point2f((KeyID%6)*33+110, KeyID/6*34+40);
    vision.Camera_Calibrate(scene, imgL, imgR);
    errcode = vision.Detect_Once(imgL);
    if (errcode != 0) {MESSAGE_ERROR("左图识别失败"+QString::number(errcode)); return;}
    errcode = vision.Point_Perspective(KeyLocate, KeyRemapL);
    if (errcode != 0) {MESSAGE_ERROR("左图映射失败"+QString::number(errcode)); return;}
    errcode = vision.Detect_Once(imgR);
    if (errcode != 0) {MESSAGE_ERROR("右图识别失败"+QString::number(errcode)); return;}
    errcode = vision.Point_Perspective(KeyLocate, KeyRemapR);
    if (errcode != 0) {MESSAGE_ERROR("右图映射失败"+QString::number(errcode)); return;}
    QString txtLocation = "";
    txtLocation += QString::number(KeyRemapL.x) + ", ";
    txtLocation += QString::number(KeyRemapL.y) + ",";
    lbl4Coordinates[1]->setText(txtLocation);
    txtLocation = "";
    txtLocation += QString::number(KeyRemapR.x) + ", ";
    txtLocation += QString::number(KeyRemapR.y) + ",";
    lbl4Coordinates[2]->setText(txtLocation);
    zhnmat::Vector3d LocateKeyCamera = vision.Cal_LocationCamera(KeyRemapL.x, KeyRemapR.x, KeyRemapL.y);  // 按键在相机坐标系下的坐标
    zhnmat::Vector3d LocateKeyWorld = vision.Cal_LocationWorld(LocateKeyCamera);  // 按键在世界坐标系下的坐标
    txtLocation = "";
    txtLocation += QString::number(LocateKeyCamera._x) + ", ";
    txtLocation += QString::number(LocateKeyCamera._y) + ", ";
    txtLocation += QString::number(LocateKeyCamera._z) + ",";
    lbl4Coordinates[3]->setText(txtLocation);
    txtLocation = "";
    txtLocation += QString::number(LocateKeyWorld._x) + ", ";
    txtLocation += QString::number(LocateKeyWorld._y) + ", ";
    txtLocation += QString::number(LocateKeyWorld._z) + ",";
    lbl4Coordinates[4]->setText(txtLocation);
    if (ckbx4Options[2]->isChecked()){
        hconcat(imgL, imgR, imgshow);
        cv::imshow("detect", imgshow);
        cv::waitKey(0);
        cv::destroyAllWindows();
    }
}

#endif

/**********************
标签5:复制粘贴按钮点击事件
**********************/
void Widget::Slot_btn5Synchronise_Click()
{
    QString str;
    std::string sstr;
    std::stringstream ss;
    int n = sender()->objectName().toInt();
    int r = n/9, c = n%9;
    if (r==0){
        switch(n) {
        case 0:  // 脉冲1复制
            for(int i=0;i<6;++i)
                str += ledt1PulseTarget[i]->text() + ",";
            ledt5clipboard[0]->setText(str);
            break;
        case 1:  // 位姿1复制
            for(int i=0;i<6;++i)
                str += lbl1Posture[i]->text() + ",";
            ledt5clipboard[1]->setText(str);
            break;
        case 2:  // 角度1复制
            for(int i=0;i<6;++i)
                str += lbl1JointAngle[i]->text() + ",";
            ledt5clipboard[2]->setText(str);
            break;
        case 3:  // 位姿4复制
            ledt5clipboard[1]->setText(lbl4Coordinates[4]->text());
            break;
        case 4:  // 脉冲1粘贴
            ss = std::stringstream(ledt5clipboard[0]->text().toStdString());
            for(int i=0;i<6;++i)
                if (getline(ss, sstr, ','))
                    ledt1PulseTarget[i]->setText(QString::fromStdString(sstr));
            break;
        case 5:  // 位姿3粘贴
            ss = std::stringstream(ledt5clipboard[1]->text().toStdString());
            for(int i=0;i<6;++i)
                if (getline(ss, sstr, ','))
                    ledt3Posture[i]->setText(QString::fromStdString(sstr));
                else
                    break;
            break;
        default: break;
        }
    }
    else if (r==1){
        if (c>=(int)ar3.PulseCalibrate.size())
            return;
        for (int i=0; i<6; ++i)
            ledt1PulseTarget[i]->setText(QString::number(ar3.PulseCalibrate[c][i]));
    }
    else if (r==2){
        if (c>=(int)ar3.PulseUser.size())
            return;
        for (int i=0; i<6; ++i)
            ledt1PulseTarget[i]->setText(QString::number(ar3.PulseUser[c][i]));
    }
}
